/*
 * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
 *
 * SPDX-License-Identifier: LGPL-2.1-or-later
 *
 * This library implements the Modbus protocol.
 * http://libmodbus.org/
 */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdarg.h>
#include <errno.h>
#include <limits.h>
#include <time.h>
#ifndef _MSC_VER
#include <unistd.h>
#endif

#include <config.h>
#include <assert.h>

#include "modbus.h"
#include "modbus-private.h"

/* Internal use */
#define MSG_LENGTH_UNDEFINED -1

/* Exported version */
const unsigned int libmodbus_version_major = LIBMODBUS_VERSION_MAJOR;
const unsigned int libmodbus_version_minor = LIBMODBUS_VERSION_MINOR;
const unsigned int libmodbus_version_micro = LIBMODBUS_VERSION_MICRO;

/* Max between RTU and TCP max adu length (so TCP) */
#define MAX_MESSAGE_LENGTH 260
//
#define _MODBUS_RTU_HEADER_LENGTH      1
#define _MODBUS_RTU_PRESET_REQ_LENGTH  6
#define _MODBUS_RTU_PRESET_RSP_LENGTH  2

#define _MODBUS_RTU_CHECKSUM_LENGTH    2
/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
 * RS232 / RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes
 */
#define MODBUS_RTU_MAX_ADU_LENGTH  256

/* 3 steps are used to parse the query */
typedef enum {
    _STEP_FUNCTION,
    _STEP_META,
    _STEP_DATA
} _step_t;

const char *modbus_strerror(int errnum) {
    switch (errnum) {
    case EMBXILFUN:
        return "Illegal function";
    case EMBXILADD:
        return "Illegal data address";
    case EMBXILVAL:
        return "Illegal data value";
    case EMBXSFAIL:
        return "Slave device or server failure";
    case EMBXACK:
        return "Acknowledge";
    case EMBXSBUSY:
        return "Slave device or server is busy";
    case EMBXNACK:
        return "Negative acknowledge";
    case EMBXMEMPAR:
        return "Memory parity error";
    case EMBXGPATH:
        return "Gateway path unavailable";
    case EMBXGTAR:
        return "Target device failed to respond";
    case EMBBADCRC:
        return "Invalid CRC";
    case EMBBADDATA:
        return "Invalid data";
    case EMBBADEXC:
        return "Invalid exception code";
    case EMBMDATA:
        return "Too many data";
    case EMBBADSLAVE:
        return "Response not from requested slave";
    default:
        return strerror(errnum);
    }
}

void _error_print(const char *context)
{
    fprintf(stderr, "ERROR %s", modbus_strerror(errno));
    if (context != NULL) {
        fprintf(stderr, ": %s\n", context);
    } else {
        fprintf(stderr, "\n");
    }
}
/* ---------------------------------------------------- */

/* Table of CRC values for high-order byte */
const uint8_t table_crc_hi[] = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};

/* Table of CRC values for low-order byte */
const uint8_t table_crc_lo[] = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
    0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
    0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
    0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
    0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
    0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
    0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
    0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
    0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
    0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
    0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
    0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
    0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
    0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
    0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
    0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
    0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
    0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};
//
uint16_t crc16(uint8_t *buffer, uint16_t buffer_length)
{
    uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
    uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
    unsigned int i; /* will index into CRC lookup */

    /* pass through message buffer */
    while (buffer_length--) {
        i = crc_hi ^ *buffer++; /* calculate the CRC  */
        crc_hi = crc_lo ^ table_crc_hi[i];
        crc_lo = table_crc_lo[i];
    }

    return (crc_hi << 8 | crc_lo);
}

int modbus_rtu_send_msg_pre(uint8_t *req, int req_length)
{
    uint16_t crc = crc16(req, req_length);
    req[req_length++] = crc >> 8;
    req[req_length++] = crc & 0x00FF;

    return req_length;
}
/* Builds a RTU request header */
int modbus_rtu_build_request_basis(int slave_id, int function,
                                    int addr, int nb,
                                    uint8_t *req)
{
    assert(slave_id != -1);
    req[0] = slave_id;
    req[1] = function;
    req[2] = addr >> 8;
    req[3] = addr & 0x00ff;
    req[4] = nb >> 8;
    req[5] = nb & 0x00ff;

    return _MODBUS_RTU_PRESET_REQ_LENGTH;
}
/* Builds a RTU response header */
int modbus_rtu_build_response_basis(int slave_id, int function, uint8_t *rsp)
{
    /* In this case, the slave is certainly valid because a check is already
     * done in _modbus_rtu_listen */
    rsp[0] = slave_id;
    rsp[1] = function;

    return _MODBUS_RTU_PRESET_RSP_LENGTH;
}
/* ---------------------------------------------------- */

/* Computes the length of the expected response */
unsigned int compute_response_length_from_request(uint8_t *req)
{
    int length;
    const int offset = _MODBUS_RTU_HEADER_LENGTH;

    switch (req[offset]) {
    case MODBUS_FC_READ_COILS:
    case MODBUS_FC_READ_DISCRETE_INPUTS: {
        /* Header + nb values (code from write_bits) */
        int nb = (req[offset + 3] << 8) | req[offset + 4];
        length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
    }
        break;
    case MODBUS_FC_WRITE_AND_READ_REGISTERS:
    case MODBUS_FC_READ_HOLDING_REGISTERS:
    case MODBUS_FC_READ_INPUT_REGISTERS:
        /* Header + 2 * nb values */
        length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
        break;
    case MODBUS_FC_READ_EXCEPTION_STATUS:
        length = 3;
        break;
    case MODBUS_FC_REPORT_SLAVE_ID:
        /* The response is device specific (the header provides the
           length) */
        return MSG_LENGTH_UNDEFINED;
    case MODBUS_FC_MASK_WRITE_REGISTER:
        length = 7;
        break;
    default:
        length = 5;
    }

    return offset + length + _MODBUS_RTU_CHECKSUM_LENGTH;
}
/*
 *  ---------- Request     Indication ----------
 *  | Client | ---------------------->| Server |
 *  ---------- Confirmation  Response ----------
 */

/* Computes the length to read after the function received */
uint8_t compute_meta_length_after_function(int function,
                                                  msg_type_t msg_type)
{
    int length;

    if (msg_type == MSG_INDICATION) {
        if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER) {
            length = 4;
        } else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS ||
                   function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS) {
            length = 5;
        } else if (function == MODBUS_FC_MASK_WRITE_REGISTER) {
            length = 6;
        } else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
            length = 9;
        } else {
            /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */
            length = 0;
        }
    } else {
        /* MSG_CONFIRMATION */
        switch (function) {
        case MODBUS_FC_WRITE_SINGLE_COIL:
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
        case MODBUS_FC_WRITE_MULTIPLE_COILS:
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            length = 4;
            break;
        case MODBUS_FC_MASK_WRITE_REGISTER:
            length = 6;
            break;
        default:
            length = 1;
        }
    }

    return length;
}

/* Computes the length to read after the meta information (address, count, etc) */
int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg,
                                          msg_type_t msg_type)
{
    int function = msg[_MODBUS_RTU_HEADER_LENGTH];
    int length;

    if (msg_type == MSG_INDICATION) {
        switch (function) {
        case MODBUS_FC_WRITE_MULTIPLE_COILS:
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            length = msg[_MODBUS_RTU_HEADER_LENGTH + 5];
            break;
        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
            length = msg[_MODBUS_RTU_HEADER_LENGTH + 9];
            break;
        default:
            length = 0;
        }
    } else {
        /* MSG_CONFIRMATION */
        if (function <= MODBUS_FC_READ_INPUT_REGISTERS ||
            function == MODBUS_FC_REPORT_SLAVE_ID ||
            function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
            length = msg[_MODBUS_RTU_HEADER_LENGTH + 1];
        } else {
            length = 0;
        }
    }

    length += _MODBUS_RTU_CHECKSUM_LENGTH;

    return length;
}


/* Waits a response from a modbus server or a request from a modbus client.
   This function blocks if there is no replies (3 timeouts).

   The function shall return the number of received characters and the received
   message in an array of uint8_t if successful. Otherwise it shall return -1
   and errno is set to one of the values defined below:
   - ECONNRESET
   - EMBBADDATA
   - EMBUNKEXC
   - ETIMEDOUT
   - read() or recv() error codes
*/


int check_confirmation(uint8_t *req, uint8_t *rsp, int rsp_length)
{
    int rc;
    int rsp_length_computed;
    const int offset = _MODBUS_RTU_HEADER_LENGTH;
    const int function = rsp[offset];

    rsp_length_computed = compute_response_length_from_request(req);

    /* Exception code */
    if (function >= 0x80) {
        if (rsp_length == (offset + 2 + (int)_MODBUS_RTU_CHECKSUM_LENGTH) &&
            req[offset] == (rsp[offset] - 0x80)) {
            /* Valid exception code received */

            int exception_code = rsp[offset + 1];
            if (exception_code < MODBUS_EXCEPTION_MAX) {
                errno = MODBUS_ENOBASE + exception_code;
            } else {
                errno = EMBBADEXC;
            }
            _error_print(NULL);
            return -1;
        } else {
            errno = EMBBADEXC;
            _error_print(NULL);
            return -1;
        }
    }

    /* Check length */
    if ((rsp_length == rsp_length_computed ||
         rsp_length_computed == MSG_LENGTH_UNDEFINED) &&
        function < 0x80) {
        int req_nb_value;
        int rsp_nb_value;

        /* Check function code */
        if (function != req[offset]) {
            fprintf(stderr,
                    "Received function not corresponding to the request (0x%X != 0x%X)\n",
                    function, req[offset]);
            errno = EMBBADDATA;
            return -1;
        }

        /* Check the number of values is corresponding to the request */
        switch (function) {
        case MODBUS_FC_READ_COILS:
        case MODBUS_FC_READ_DISCRETE_INPUTS:
            /* Read functions, 8 values in a byte (nb
             * of values in the request and byte count in
             * the response. */
            req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
            req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
            rsp_nb_value = rsp[offset + 1];
            break;
        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
        case MODBUS_FC_READ_HOLDING_REGISTERS:
        case MODBUS_FC_READ_INPUT_REGISTERS:
            /* Read functions 1 value = 2 bytes */
            req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
            rsp_nb_value = (rsp[offset + 1] / 2);
            break;
        case MODBUS_FC_WRITE_MULTIPLE_COILS:
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            /* N Write functions */
            req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
            rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
            break;
        case MODBUS_FC_REPORT_SLAVE_ID:
            /* Report slave ID (bytes received) */
            req_nb_value = rsp_nb_value = rsp[offset + 1];
            break;
        default:
            /* 1 Write functions & others */
            req_nb_value = rsp_nb_value = 1;
        }

        if (req_nb_value == rsp_nb_value) {
            rc = rsp_nb_value;
        } else {
            fprintf(stderr,
                    "Quantity not corresponding to the request (%d != %d)\n",
                    rsp_nb_value, req_nb_value);
            errno = EMBBADDATA;
            rc = -1;
        }
    } else {
        fprintf(stderr,
                "Message length not corresponding to the computed length (%d != %d)\n",
                rsp_length, rsp_length_computed);
        errno = EMBBADDATA;
        rc = -1;
    }

    return rc;
}

int response_io_status(uint8_t *tab_io_status,
                              int address, int nb,
                              uint8_t *rsp, int offset)
{
    int shift = 0;
    /* Instead of byte (not allowed in Win32) */
    int one_byte = 0;
    int i;

    for (i = address; i < address + nb; i++) {
        one_byte |= tab_io_status[i] << shift;
        if (shift == 7) {
            /* Byte is full */
            rsp[offset++] = one_byte;
            one_byte = shift = 0;
        } else {
            shift++;
        }
    }

    if (shift != 0)
        rsp[offset++] = one_byte;

    return offset;
}
/* Reads IO status */
int read_io_status(int slave_id, int function, int addr, int nb, uint8_t *req)
{
    if(NULL == req){
        return -1;
    }

    return modbus_rtu_build_request_basis(slave_id, function, addr, nb, req);
}

/* Reads the boolean status of bits and sets the array elements
   in the destination to TRUE or FALSE (single bits). */
int get_modbus_read_bits_msg(int slave_id, int addr, int nb, uint8_t *dest, int dst_len)
{
    int rc = -1;
    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (_MIN_REQ_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_READ_BITS) {
        fprintf(stderr,
                "ERROR Too many bits requested (%d > %d)\n",
                nb, MODBUS_MAX_READ_BITS);
        errno = EMBMDATA;
        return -1;
    }

    rc = read_io_status(slave_id, MODBUS_FC_READ_COILS, addr, nb, dest);
    if (rc == -1)
        return -1;
    else
        rc = modbus_rtu_send_msg_pre(dest, rc);
        return rc;
}


/* Same as modbus_read_bits but reads the remote device input table */
int get_modbus_read_input_bits_msg(int slave_id, int addr, int nb, uint8_t *dest, int dst_len)
{
    int rc = -1;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (_MIN_REQ_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_READ_BITS) {
        fprintf(stderr,
                "ERROR Too many discrete inputs requested (%d > %d)\n",
                nb, MODBUS_MAX_READ_BITS);
        errno = EMBMDATA;
        return -1;
    }

    rc = read_io_status(slave_id, MODBUS_FC_READ_DISCRETE_INPUTS, addr, nb, dest);

    if (rc == -1)
        return -1;
    else
        rc = modbus_rtu_send_msg_pre(dest, rc);
        return rc;
}

/* Reads the data from a remove device and put that data into an array */
int read_registers(int slave_id, int function, int addr, int nb, uint8_t *dest)
{
    int rc = -1;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_READ_REGISTERS) {
            fprintf(stderr,
                    "ERROR Too many registers requested (%d > %d)\n",
                    nb, MODBUS_MAX_READ_REGISTERS);
        errno = EMBMDATA;
        return -1;
    }

    rc = modbus_rtu_build_request_basis(slave_id, function, addr, nb, dest);
    rc = modbus_rtu_send_msg_pre(dest, rc);
    return rc;
}

/* Reads the holding registers of remote device and put the data into an
   array */
int get_modbus_read_registers_msg(int slave_id, int addr, int nb, uint8_t *dest, int dst_len)
{
    int rc = -1;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (_MIN_REQ_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_READ_REGISTERS) {
        fprintf(stderr,
                "ERROR Too many registers requested (%d > %d)\n",
                nb, MODBUS_MAX_READ_REGISTERS);
        errno = EMBMDATA;
        return -1;
    }

    rc = read_registers(slave_id, MODBUS_FC_READ_HOLDING_REGISTERS, addr, nb, dest);
    return rc;
}

/* Reads the input registers of remote device and put the data into an array */
int get_modbus_read_input_registers_msg(int slave_id, int addr, int nb, uint8_t *dest, int dst_len)
{
    int rc = -1;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (_MIN_REQ_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_READ_REGISTERS) {
        fprintf(stderr,
                "ERROR Too many input registers requested (%d > %d)\n",
                nb, MODBUS_MAX_READ_REGISTERS);
        errno = EMBMDATA;
        return -1;
    }

    rc = read_registers(slave_id, MODBUS_FC_READ_INPUT_REGISTERS,
                            addr, nb, dest);
    return rc;
}

/* Write a value to the specified register of the remote device.
   Used by write_bit and write_register */
int write_single(int slave_id, int function, int addr, const uint16_t value, uint8_t *dest)
{
    int rc = -1;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest)){
        errno = EINVAL;
        return -1;
    }

    rc = modbus_rtu_build_request_basis(slave_id, function, addr, (int) value, dest);
    return rc;
}

/* Turns ON or OFF a single bit of the remote device */
int get_modbus_write_bit_msg(int slave_id, int coil_addr, int status, uint8_t *dest, int dst_len)
{
    int rc = -1;
    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (1 > dst_len)){
        errno = EINVAL;
        return -1;
    }
    rc = write_single(slave_id, MODBUS_FC_WRITE_SINGLE_COIL, coil_addr, status ? 0xFF00 : 0, dest);
    rc = modbus_rtu_send_msg_pre(dest, rc);
    return rc;
}

/* Writes a value in one register of the remote device */
int get_modbus_write_register_msg(int slave_id, int addr, const uint16_t value, uint8_t *dest, int dst_len)
{
    int rc = -1;
    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (_MIN_REQ_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }
    rc = write_single(slave_id, MODBUS_FC_WRITE_SINGLE_REGISTER, addr, value, dest);
    rc = modbus_rtu_send_msg_pre(dest, rc);
    return rc;
}

/* Write the bits of the array in the remote device */
int get_modbus_write_bits_msg(int slave_id, int addr, int nb, const uint8_t *src, uint8_t *dest, int dst_len)
{
    int i;
    int byte_count;
    int req_length;
    int bit_check = 0;
    int pos = 0;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (MAX_MESSAGE_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_WRITE_BITS) {
        fprintf(stderr, "ERROR Writing too many bits (%d > %d)\n",  nb, MODBUS_MAX_WRITE_BITS);
        errno = EMBMDATA;
        return -1;
    }

    req_length = modbus_rtu_build_request_basis(slave_id,
                                                MODBUS_FC_WRITE_MULTIPLE_COILS,
                                                addr, nb, dest);
    byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
    dest[req_length++] = byte_count;

    for (i = 0; i < byte_count; i++) {
        int bit;

        bit = 0x01;
        dest[req_length] = 0;

        while ((bit & 0xFF) && (bit_check++ < nb)) {
            if (src[pos++])
                dest[req_length] |= bit;
            else
                dest[req_length] &=~ bit;

            bit = bit << 1;
        }
        req_length++;
    }
    req_length = modbus_rtu_send_msg_pre(dest, req_length);
    return req_length;
}

/* Write the values from the array to the registers of the remote device */
int get_modbus_write_registers_msg(int slave_id, int addr, int nb, const uint16_t *src, uint8_t *dest, int dst_len)
{
    int req_length;
    int byte_count;
    
    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (MAX_MESSAGE_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (nb > MODBUS_MAX_WRITE_REGISTERS) {
        fprintf(stderr,
                "ERROR Trying to write to too many registers (%d > %d)\n",
                nb, MODBUS_MAX_WRITE_REGISTERS);
        errno = EMBMDATA;
        return -1;
    }

    req_length = modbus_rtu_build_request_basis(slave_id,
                                                MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
                                                addr, nb, dest);
    byte_count = nb * 2;
    dest[req_length++] = byte_count;
    memcpy(dest+req_length, src, byte_count);
    req_length += byte_count;
    req_length = modbus_rtu_send_msg_pre(dest, req_length);
    return req_length;
}

int get_modbus_mask_write_register_msg(int slave_id, int addr, uint16_t and_mask, uint16_t or_mask, uint8_t *dest, int dst_len)
{
    int req_length;
    /* The request length can not exceed _MIN_REQ_LENGTH - 2 and 4 bytes to
     * store the masks. The ugly subtraction is there to remove the 'nb' value
     * (2 bytes) which is not used. */
    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || ((_MIN_REQ_LENGTH + 2) > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    req_length = modbus_rtu_build_request_basis(slave_id,
                                                   MODBUS_FC_MASK_WRITE_REGISTER,
                                                   addr, 0, dest);

    /* HACKISH, count is not used */
    req_length -= 2;

    dest[req_length++] = and_mask >> 8;
    dest[req_length++] = and_mask & 0x00ff;
    dest[req_length++] = or_mask >> 8;
    dest[req_length++] = or_mask & 0x00ff;
    req_length = modbus_rtu_send_msg_pre(dest, req_length);
    return req_length;
}

/* Write multiple registers from src array to remote device and read multiple
   registers from remote device to dest array. */
int get_modbus_write_and_read_registers_msg(int slave_id,
                                    int write_addr, int write_nb,
                                    const uint16_t *src,
                                    int read_addr, int read_nb,
                                    uint8_t *dest, int dst_len)

{
    int req_length;
    int i;
    int byte_count;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (MAX_MESSAGE_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    if (write_nb > MODBUS_MAX_WR_WRITE_REGISTERS) {
        fprintf(stderr,
                "ERROR Too many registers to write (%d > %d)\n",
                write_nb, MODBUS_MAX_WR_WRITE_REGISTERS);
        errno = EMBMDATA;
        return -1;
    }

    if (read_nb > MODBUS_MAX_WR_READ_REGISTERS) {
        fprintf(stderr,
                "ERROR Too many registers requested (%d > %d)\n",
                read_nb, MODBUS_MAX_WR_READ_REGISTERS);
        errno = EMBMDATA;
        return -1;
    }
    req_length = modbus_rtu_build_request_basis(slave_id,
                                                MODBUS_FC_WRITE_AND_READ_REGISTERS,
                                                read_addr, read_nb, dest);

    dest[req_length++] = write_addr >> 8;
    dest[req_length++] = write_addr & 0x00ff;
    dest[req_length++] = write_nb >> 8;
    dest[req_length++] = write_nb & 0x00ff;
    byte_count = write_nb * 2;
    dest[req_length++] = byte_count;

    for (i = 0; i < write_nb; i++) {
        dest[req_length++] = src[i] >> 8;
        dest[req_length++] = src[i] & 0x00FF;
    }
    req_length = modbus_rtu_send_msg_pre(dest, req_length);
    return req_length;
}

/* Send a request to get the slave ID of the device (only available in serial
   communication). */
int get_modbus_report_slave_id(int slave_id, int max_dest, uint8_t *dest, int dst_len)
{
    int req_length;

    if ((slave_id < 0) || (slave_id > 254) || (NULL == dest) || (_MIN_REQ_LENGTH > dst_len)) {
        errno = EINVAL;
        return -1;
    }

    req_length = modbus_rtu_build_request_basis(slave_id, MODBUS_FC_REPORT_SLAVE_ID, 0, 0, dest);

    /* HACKISH, addr and count are not used */
    req_length -= 4;
    req_length = modbus_rtu_send_msg_pre(dest, req_length);
    return req_length;
}

int modbus_get_header_length()
{
    return _MODBUS_RTU_HEADER_LENGTH;
}

int modbus_get_data_len(uint8_t *data){
    return data[_MODBUS_RTU_HEADER_LENGTH+1];
}